#include <iostream>
#include "HRLocomotion/hr_locomotion.h"

using std::cout;
using std::endl;

using hrLocomotion::VectorXr;
int main(){
    std::cout << "Starting test." << std::endl;
    hrLocomotion::MPCLocomotion mpc = hrLocomotion::MPCLocomotion();

    std::cout << "Everything is running fine." << std::endl;
    if(mpc.init()) std::cout << "QPSolved" << std::endl;
    hrLocomotion::VectorXr u = mpc.getU();

    std::cout << "Control" << std::endl << u << std::endl;

    mpc.solveNext();
    u = mpc.getU();

    std::cout << "Control" << std::endl << u << std::endl;

    hrLocomotion::RobotState state;
    state.setOnes();
    std::cout << "Robot state" << std::endl << state << std::endl;
    state = state * 2;
    std::cout << "Robot state" << std::endl << state << std::endl;

    hrLocomotion::MatrixXr id = hrLocomotion::MatrixXr::Identity(9,9);
    state = 2*id*state;
    std::cout << "Robot state" << std::endl << state << std::endl;

    hrLocomotion::VectorXr x_new(3);
    hrLocomotion::VectorXr y_new(3);
    hrLocomotion::VectorXr theta_new(3);
    x_new << 0.5 , 0.6 , 0.7;
    y_new << 1.5 , 1.6 , 1.7;
    theta_new << 2.5 , 2.6 , 2.7;

    state.setX(x_new);
    std::cout << "Robot state" << std::endl << state << std::endl;

    state.setY(y_new);
    std::cout << "Robot state" << std::endl << state << std::endl;

    state.setTheta(theta_new);
    std::cout << "Robot state" << std::endl << state << std::endl;

    VectorXr s = state;

    std::cout << "Robot state" << std::endl << s << std::endl;

    std::cout << "Starting test -- MPC constructor" << std::endl;

    hrLocomotion::MPCData data_01;
    data_01.printConstants();

    hrLocomotion::MPCData data_02 = hrLocomotion::MPCData(0.5);
    data_02.printConstants();

    hrLocomotion::MPCData data_03 = hrLocomotion::MPCData(0.5, 0.2);
    data_03.printConstants();


    hrLocomotion::MPCData data_04 = hrLocomotion::MPCData(0.5, 0.2, 0.1,0.2,0.3);
    data_04.printConstants();


    hrLocomotion::MPCData data_05 = hrLocomotion::MPCData(0.5, 0.2, 3,10);
    data_05.printConstants();

    hrLocomotion::MPCData data_06 = hrLocomotion::MPCData(0.5, 0.2, 3,10);
    data_06.printConstants();

    VectorXr gains(4);
    gains << 0.11 , 0.12 , 0.13,0.16;
    hrLocomotion::MPCData data_07 = hrLocomotion::MPCData(0.5, 0.2, 4, 11, gains);
    data_07.printConstants();



    hrLocomotion::MPCData data_10 = hrLocomotion::MPCData(0.3,0.1,2,5);
    data_10.init();     //0

    /*
    cout << "Hessian " <<  data_10.getStage() << " : "<< endl << data_10.getHessian() << endl;
    data_10.next();     //1
    cout << "Hessian " <<  data_10.getStage() << " : "<< endl << data_10.getHessian() << endl;
    data_10.next();     //2
    cout << "Hessian " <<  data_10.getStage() << " : "<< endl << data_10.getHessian() << endl;
    data_10.next();     //3
    cout << "Hessian " <<  data_10.getStage() << " : "<< endl << data_10.getHessian() << endl;
    data_10.next();     //4
    cout << "Hessian " <<  data_10.getStage() << " : "<< endl << data_10.getHessian() << endl;
    data_10.next();     //0
    cout << "Hessian " <<  data_10.getStage() << " : "<< endl << data_10.getHessian() << endl;
    data_10.next();     //1
    cout << "Hessian " <<  data_10.getStage() << " : "<< endl << data_10.getHessian() << endl;
    data_10.next();     //2
    cout << "Hessian " <<  data_10.getStage() << " : "<< endl << data_10.getHessian() << endl;
    data_10.next();     //3
    cout << "Hessian " <<  data_10.getStage() << " : "<< endl << data_10.getHessian() << endl;
    data_10.next();     //4
    cout << "Hessian " <<  data_10.getStage() << " : "<< endl << data_10.getHessian() << endl;
    */
    cout << "FootState" << endl;
    hrLocomotion::VectorXr footState(3);
    footState << 0.5,0.5,0.0;
    cout << "RState" << endl;
    hrLocomotion::RobotState rState;
    cout << "State size: " << rState.rows() << endl;
    hrLocomotion::Vector3r xk;
    hrLocomotion::Vector3r yk;
    hrLocomotion::Vector3r thetak;
    int N_h = 5*2;  // number_of_steps * samples_per_step
    cout << "Init vectors" << endl;
    xk << 0.5 , 0.1, 0.0;
    yk << -0.5, 0.1 , 0.0;
    thetak << 0.0, 0.0 ,0.0;
    VectorXr x_vel(N_h);
    VectorXr y_vel(N_h);
    x_vel.setOnes();
    y_vel.setOnes();
    cout << "SetState" << endl << xk << endl << yk << endl << thetak << endl;
    rState.setState(xk,yk,thetak);

    cout << "F vector" << endl << data_10.getFvector(rState,footState,x_vel,y_vel) << endl;



}
